/*
 * BranchAndBound.c
 *
 *  Created on: 18/mar/2013
 *      Authors: Ialenti Andrea, Marangon Fabio
 */

#include "../headers/Graph.h"
#include "../headers/Lagrangian.h"
#include "../headers/OneTree.h"
#include "../headers/Printer.h"
#include "../headers/Utility.h"

#include <stdlib.h>
#include <string.h>

//	Function used to create random lagrangian coefficients 'u'.
Status randomizeU(double *u, double *uO, int size, int *s);

Status computeLagrRlx(const Config *cfg, Graph *g, funcLu *f, bool verbose) {
	char *funcName = initCharArray(cfg->NAME_LEN);
	sprintf(funcName, "computeLagrRlx");

	srand(g->size);
	strcpy(f->name, g->name);
    int i, j, randCnt = 0, alphaCnt = 1;
    time_t t1, t2;

    //	Initialize memory for the 1-tree and the function 'L(u)'.
    OneTree t;
    bool init = initOneTree(cfg, &t, g->size);
    if (!init) {
		fprintf(stdout, "***'%s': 1-tree memory initialization error.\n", funcName);
		freeCharArray(funcName);
		disposeOneTree(&t);
		return ERR_MEM_ALLOC;
	}

    //	Initialize the vector 'u(0)' to zero.
    double sum_U = 0;
    double *uK = initDblArray(g->size);
    double *uK_old = initDblArray(g->size);
    for (i = 0; i < g->size; i++)
    	uK[i] = uK_old[i] = 0;

    //	Set the upper bound and the starting 'alpha'.
    f->upperBound = g->max;
    double alpha = 2;
    
    //	Compute the first 1-tree to find 'L(0)'.
    computeOneTree(cfg, g, &t);
    f->L[0] = t.totalCost;

    //	Initialize the first sub-gradient vector.
    int *subGrad = initIntArray(g->size);
    for (i = 0; i < t.size; i++)
    	subGrad[i] = t.nodes[i].degree - 2;

    //	Compute the first squared norm.
    double normSquare = 0;
    for (j = 0; j < t.size; j++)
    	normSquare += (subGrad[j] * subGrad[j]);

    //	Compute the first step length (increase the upper bound by 25% until it is greater than the current 'L(0)').
    double stepT = 0;
    do {
    	stepT = alpha * ((f->upperBound - f->L[0]) / normSquare);
    	f->upperBound = (stepT <= 0) ? f->upperBound * cfg->UB_INC : f->upperBound;
    } while (stepT <= 0);

    //	Compute the relaxation for at most a limited number of iterations.
    time(&t1);
    for (i = 1; i < f->size; i++) {

    	//	Restore original graph costs and apply the current lagrangian coefficients.
    	applyLagrCoeffs(cfg, g, uK);

		//	Recompute the 1-tree from the graph modified with the new costs.
		computeOneTree(cfg, g, &t);

		//	Re-compute the sub-gradient and update 'u' for each node in the graph.
		for (j = 0, sum_U = 0; j < t.size; j++) {

			subGrad[j] = t.nodes[j].degree - 2;
			if (subGrad[j] != 0)
				uK[j] = uK_old[j] + stepT * subGrad[j];

			sum_U += uK_old[j];
		}

		//	Re-compute the squared norm.
		for (j = 0, normSquare = 0; j < t.size; j++)
			normSquare += (subGrad[j] * subGrad[j]);

		//	Store the current value of 'L(u)'.
		f->L[i] = t.totalCost - 2 * sum_U;

		//	When the squared norm is zero 'L(u)' is the optimal solution for the TSP.
		if (!(normSquare > -cfg->EPS && normSquare < cfg->EPS)) {

			//	Update the step length, increasing the upper bound if necessary.
			do {
				stepT = alpha * ((f->upperBound - f->L[i]) / normSquare);
				f->upperBound = (stepT <= 0) ? f->upperBound * cfg->UB_INC : f->upperBound;
			} while (stepT <= 0);
		}

		//	Update the final vector 'u' (if necessary) and stop the relaxation procedure.
		else {
			if (f->best < f->L[i]) {
				f->best = f->L[i];
				f->kBest = i;
				for(j = 0; j < t.size; j++)
					g->u[j] = uK_old[j];
			}
			f->size = i;
			applyLagrCoeffs(cfg, g, g->u);

			//	Free memory and exit.
			freeCharArray(funcName);
			disposeOneTree(&t);
			freeDblArray(uK);
			freeDblArray(uK_old);
			freeIntArray(subGrad);
			return SUCCESS;
		}

		//	Count how many iterations have occurred without any improvement in the 'L(0)'.
		if (f->best >= f->L[i])
			alphaCnt++;

		//	If there was an improvement register it and reset the counter.
		else {
			f->best = f->L[i];
			f->kBest = i;
			for(j = 0; j < t.size; j++)
				g->u[j] = uK_old[j];
			alphaCnt = 1;
		}

		//	If the function has no significative variation from the best value, update the counter, otherwise reset it.
		if (f->L[i] > 0.99 * f->best && f->L[i] < 1.01 * f->best)
			randCnt++;
		else
			randCnt = 1;

		//	If there was no variation for a enough steps then randomize the vector 'u'.
		if (randCnt % cfg->RAND_STEP == 0)
			randomizeU(uK, uK_old, g->size, subGrad);

		//	If there was no improvement in the last 'step' iterations, divide 'alpha' by 2.
		alpha = (alphaCnt % f->step == 0) ? alpha / 2 : alpha;

		//	Store the current 'uK' for the next iteration.
		for(j = 0; j < g->size; j++)
			uK_old[j] = uK[j];

		if (verbose && i % 500 == 0) {
			time(&t2);
			fprintf(stdout, "\t\t'%s': iteration # %d completed in  %0.2f second(s) (%0.2f second(s) normalized time). Best bound: %0.2f, continuing. . .\n", funcName, i, difftime(t2, t1), difftime(t2, t1) * cfg->NORM_TIME, f->best);
			fflush(stdout);
		}
    }
    applyLagrCoeffs(cfg, g, g->u);

    //	Free memory.
    freeCharArray(funcName);
    disposeOneTree(&t);
    freeDblArray(uK);
    freeDblArray(uK_old);
    freeIntArray(subGrad);
    return SUCCESS;
}

Status applyLagrCoeffs(const Config *cfg, Graph *g, double *u) {
	int i, j;

	//	Restore the original costs and apply the lagrangian coefficients.
	restoreOrigCosts(g);

	for (i = 0, g->max = 0; i < g->size - 1; i++)
		for (j = i + 1; j < g->size; j++) {
			if (g->edges[i][j].cost < cfg->INF && g->edges[j][i].cost < cfg->INF && !(g->edges[i][j].cost > -cfg->EPS && g->edges[j][i].cost < cfg->EPS))
				g->edges[i][j].cost = g->edges[j][i].cost += u[i] + u[j];

			if (g->max < g->edges[i][j].cost)
				g->max = g->edges[i][j].cost;
		}
	return SUCCESS;
}

Status randomizeU (double *u, double * uO, int size, int *s) {
	int i;
	for (i = 0; i < size; i++)
		uO[i] = u[i] = uO[i] + (rand() / (double)RAND_MAX) * s[i];
	return SUCCESS;
}

Status rmGraphEdges(const Config *cfg, Graph *g, double LB, double UB) {
	char *funcName = initCharArray(cfg->NAME_LEN);
	sprintf(funcName, "rmGraphEdges");

	//	Various indices and counters.
	int i, j, k, m, n, c1 = 0, c2 = 0, cnt = 0;

	//	Create the lagrangian 1-tree.
	OneTree t;
	bool init = initOneTree(cfg, &t, g->size);
	if (!init) {
		fprintf(stdout, "***'%s': 1-tree memory initialization error.\n", funcName);
		freeCharArray(funcName);
		disposeOneTree(&t);
		return ERR_MEM_ALLOC;
	}

	computeOneTree(cfg, g, &t);

	//	Calculate the limit.
	double lim = UB - LB;
	double delta = 0;

	//	Define variables used to find the cycles.
	Edge *e1 = NULL;
	Edge *e2 = NULL;
	int *nodeIDs1 = initIntArray(t.size);
	int *nodeIDs2 = initIntArray(t.size);
	double *maxCosts1 = initDblArray(t.size);
	double *maxCosts2 = initDblArray(t.size);
	double max1 = 0, max2 = 0;
	double max = 0;

	//	Check all the edges, skipping those edges that form the 1-tree (node 0 is excluded).
	for(i = 1; i < g->size - 1; i++) {
		for(j = i + 1; j < g->size; j++, c1 = 0, c2 = 0, max1 = 0, max2 = 0, max = 0) {

			if (t.costs[i][j] > -cfg->EPS && t.costs[i][j] < cfg->EPS) {

				//	Find the two 1-tree edges connected to the current edge (skip edges e0, e1).
				for (k = 2; k < t.size; k++)
					if (t.edges[k].end->id == g->edges[i][j].start->id)
						e1 = &t.edges[k];
					else if (t.edges[k].end->id == g->edges[i][j].end->id)
						e2 = &t.edges[k];

				//	Go from 'e1' to the root, storing the max-cost edge for each step.
				if(e1 != NULL)
					while (e1->prev != e1) {

						//	Store IDs and max-costs.
						nodeIDs1[c1] = e1->start->id;
						if (e1->cost > max1)
							max1 = e1->cost;
						maxCosts1[c1] = max1;

						//	Go to the next edge.
						e1 = e1->prev;
						c1++;
					}

				//	Go from 'e2' to the root, storing the max1-cost edge for each step.
				if(e2 != NULL)
					while (e2->prev != e2) {

						//	Store IDs and max-costs.
						nodeIDs2[c2] = e2->start->id;
						if (e2->cost > max2)
							max2 = e2->cost;
						maxCosts2[c2] = max2;

						//	Go to the next edge.
						e2 = e2->prev;
						c2++;
					}

				//	Find the first common ancestor for the two edges.
				for(m = c1, n = c2; m > 0 && n > 0; m--, n--)
					if (nodeIDs1[m] != nodeIDs2[n])
						break;

				//	Find the max cost edge from 'e1' to the common ancestor.
				for (c1 = 0; c1 < m; c1++)
					if (maxCosts1[c1] > max)
						max = maxCosts1[c1];

				//	Find the max cost edge from 'e2' to the common ancestor.
				for (c1 = 0; c1 < n; c1++)
					if (maxCosts2[c1] > max)
						max = maxCosts2[c1];

				//	Set 'delta' using the current edge and the max-cost edge in the cycle.
				delta = g->edges[i][j].cost - max;

				//	If delta is big enough and the nodes have enough edges, remove the edge from the graph.
				if (delta >= lim && g->nodes[i].degree > 3 && g->nodes[j].degree > 3) {
					rmEdge(g, i, j);
					cnt++;
				}
			}
		}
	}
	g->edgeCnt -= cnt;

	//	Free memory.
	freeCharArray(funcName);
	disposeOneTree(&t);
	freeIntArray(nodeIDs1);
	freeIntArray(nodeIDs2);
	freeDblArray(maxCosts1);
	freeDblArray(maxCosts2);
	return SUCCESS;
}
